#include "filter.h"

/* --------------------------------------- 基础滤波算法 --------------------------------------- */

/**
 * @brief 限幅滤波算法
 * @param curValue 当前值
 * @param preValue 上一次的值
 * @param maxLimit 限幅范围
 * @return         当在限幅范围内时，返回当前值；
 *                 当在限幅范围外时，返回上一次的值；
 */
uint16_t limitFilter(const uint16_t curValue, const uint16_t preValue, const uint16_t maxLimit)
{
    uint16_t diff = (curValue > preValue) ? (curValue - preValue) : (preValue - curValue);
    if (diff > maxLimit)
        return preValue;
    else
        return curValue;
}

/**
 * @brief 中值滤波算法
 * @param pValueArray 待滤波的数组（会被修改）
 * @param length      待滤波数组的长度
 * @return            返回数组的中间值
 */
uint16_t medianFilter(uint16_t pValueArray[], const uint8_t length)
{
    if (length == 0) return 0;
    if (length == 1) return pValueArray[0];
    
    uint16_t tempValue = 0;
    
    /* 使用冒泡法排序（从小到大） */
    for (uint8_t i = 0; i < length - 1; ++i)
    {
        for (uint8_t j = 0; j < length - 1 - i; ++j)
        {
            if (pValueArray[j] > pValueArray[j + 1])
            {
                tempValue = pValueArray[j];
                pValueArray[j] = pValueArray[j + 1];
                pValueArray[j + 1] = tempValue;
            }
        }
    }
    
    /* 返回中间值 */
    if (length % 2 == 0)
    {
        tempValue = (pValueArray[length/2 - 1] + pValueArray[length/2]) / 2;
    }
    else
    {
        tempValue = pValueArray[length / 2];
    }
    
    return tempValue;
}

/**
 * @brief 平均值滤波算法
 * @param pValueArray 待滤波的数组
 * @param length      待滤波数组的长度
 * @return            返回待滤波数组的算数平均值
 */
uint16_t aveFilter(const uint16_t pValueArray[], const uint8_t length)
{
    if (length == 0) return 0;
    
    uint16_t sum = 0;
    
    /* 求和 */
    for (uint8_t i = 0; i < length; ++i)
    {
        sum += pValueArray[i];
    }
    
    return sum / length;
}

/* ------------------------------------- 滑动窗口滤波算法 ------------------------------------- */

/**
 * @brief 中位数值滤波算法（去极值平均滤波）
 * @param pValueArray  待滤波的数组
 * @param length       待滤波数组的长度
 * @return             返回数组去掉最大值和最小值之后数据的平均值
 */
uint16_t midAveFilter(uint16_t pValueArray[], const uint8_t length)
{
    if (length <= 2) return (length == 0) ? 0 : pValueArray[0];
    
    uint16_t tempValue = 0;
    uint16_t sum = 0;
    
    /* 使用冒泡法排序（从小到大） */
    for (uint8_t i = 0; i < length - 1; ++i)
    {
        for (uint8_t j = 0; j < length - 1 - i; ++j)
        {
            if (pValueArray[j] > pValueArray[j + 1])
            {
                tempValue = pValueArray[j];
                pValueArray[j] = pValueArray[j + 1];
                pValueArray[j + 1] = tempValue;
            }
        }
    }
    
    /* 去掉最大值和最小值后求和 */
    for (uint8_t i = 1; i < length - 1; ++i)
    {
        sum += pValueArray[i];
    }
    
    return sum / (length - 2);
}

/**
 * @brief 滑动平均滤波算法
 * @param pValueArray 待滤波的数组
 * @param length      待滤波数组的长度
 * @param newValue    最新的数据
 * @return            返回滑动之后数据的平均值
 */
uint16_t movAVeFilter(uint16_t pValueArray[], const uint8_t length, const uint16_t newValue)
{
    if (length == 0) return newValue;
    if (length == 1) 
    {
        pValueArray[0] = newValue;
        return newValue;
    }
    
    uint16_t sum = 0;
    
    /* 数据向左滑动一位 */
    for (uint8_t i = 0; i < length - 1; ++i)
    {
        pValueArray[i] = pValueArray[i + 1];
        sum += pValueArray[i];
    }
    
    /* 添加新值到最后一位 */
    pValueArray[length - 1] = newValue;
    sum += newValue;
    
    return sum / length;
}

/**
 * @brief 限幅滑动平均
 * @param pValueArray 待滤波的数组
 * @param length      待滤波数组的长度
 * @param newValue    最新的数据
 * @param maxLimit    限幅范围
 * @return            返回限幅滑动之后数据的平均值
 */
uint16_t limitMovAve(uint16_t pValueArray[], const uint8_t length, const uint16_t newValue, const uint16_t maxLimit)
{
    if (length == 0) return newValue;
    if (length == 1) 
    {
        pValueArray[0] = newValue;
        return newValue;
    }
    
    uint16_t sum = 0;
    uint16_t filteredValue = newValue;
    
    /* 对最新数据进行限幅检查 */
    uint16_t diff = (pValueArray[length - 1] > newValue) ? (pValueArray[length - 1] - newValue) : (newValue - pValueArray[length - 1]);
    if (diff > maxLimit)
    {
        filteredValue = pValueArray[length - 1];  /* 超出限幅，使用上一个值 */
    }
    
    /* 数据向左滑动一位 */
    for (uint8_t i = 0; i < length - 1; ++i)
    {
        pValueArray[i] = pValueArray[i + 1];
        sum += pValueArray[i];
    }
    
    /* 添加限幅后的新值到最后一位 */
    pValueArray[length - 1] = filteredValue;
    sum += filteredValue;
    
    return sum / length;
}

/**
 * @brief 加权递推平均滤波法
 * @param weight      存放权重的数组
 * @param pValueArray 待滤波的数组
 * @param length      待滤波数组的长度
 * @param newValue    新的数据值
 * @return            返回滤波后的数据
 */
uint16_t weightMovAve(const uint8_t weight[], uint16_t pValueArray[], const uint8_t length, const uint16_t newValue)
{
    if (weight == NULL || pValueArray == NULL || length == 0) return newValue;
    
    uint16_t weightSum = 0;
    uint32_t sum = 0;
    
    /* 获取权重的和 */
    for (uint8_t i = 0; i < length; ++i)
    {
        weightSum += weight[i];
    }
    
    if (weightSum == 0) return newValue;  /* 防止除零错误 */
    
    /* 数据向左滑动一位 */
    for (uint8_t i = 0; i < length - 1; ++i)
    {
        pValueArray[i] = pValueArray[i + 1];
    }
    
    /* 更新最后的数据 */
    pValueArray[length - 1] = newValue;
    
    /* 加权求和 */
    for (uint8_t i = 0; i < length; ++i)
    {
        sum += (uint32_t)pValueArray[i] * weight[i];
    }
    
    return sum / weightSum;
}

/* -------------------------------------- 高级滤波算法 -------------------------------------- */

/**
 * @brief 一阶低通滤波算法
 * @param curValue  当前值
 * @param preValue  上一次的值
 * @param k         系数 (0.0 < k <= 1.0)
 * @return          返回滤波之后的数据
 */
uint16_t lowPass1st(const uint16_t curValue, const uint16_t preValue, const double k)
{
    if (k <= 0.0 || k > 1.0) return curValue;  /* 参数检查 */
    
    double tempCurValue = (double)curValue;
    double tempPreValue = (double)preValue;
    double tempValue = 0.0;
    
    /* 计算滤波数值 */
    tempValue = tempCurValue * k + (1.0 - k) * tempPreValue;
    
    return (uint16_t)(tempValue + 0.5);  /* 四舍五入 */
}

/**
 * @brief 消除抖动算法
 * @param curValue  当前值
 * @param preValue  上一次的值
 * @param pCount    抖动累计次数指针（静态变量）
 * @param threshold 抖动阈值
 * @return          返回消除抖动之后的值
 */
uint16_t clearJitter(const uint16_t curValue, const uint16_t preValue, uint8_t *pCount, const uint8_t threshold)
{
    if (pCount == NULL) return curValue;
    
    if (curValue != preValue)
    {
        /* 累计当前数据不相等的次数 */
        (*pCount)++;
        if (*pCount > threshold)
        {
            *pCount = 0;  /* 重置计数器 */
            return curValue;  /* 确认变化，返回新值 */
        }
        else
        {
            return preValue;  /* 抖动期间，保持旧值 */
        }
    }
    else
    {
        /* 若相等则清零 */
        *pCount = 0;
        return curValue;
    }
}

/**
 * @brief 限幅消除抖动算法
 * @param curValue  当前值
 * @param preValue  上一次的值
 * @param pCount    抖动累计次数指针（静态变量）
 * @param maxLimit  限幅范围
 * @param threshold 抖动阈值
 * @return          返回限幅消除抖动之后的值
 */
uint16_t limitClearJitter(const uint16_t curValue, const uint16_t preValue, uint8_t *pCount, const uint16_t maxLimit, const uint8_t threshold)
{
    if (pCount == NULL) return curValue;
    
    uint16_t newValue = curValue;
    
    /* 对最新的数据进行限幅 */
    uint16_t diff = (curValue > preValue) ? (curValue - preValue) : (preValue - curValue);
    if (diff > maxLimit)
    {
        newValue = preValue;  /* 超出限幅，使用上一个值 */
    }
    
    /* 对当前数据进行消抖 */
    if (newValue != preValue)
    {
        /* 累计当前数据不相等的次数 */
        (*pCount)++;
        if (*pCount > threshold)
        {
            *pCount = 0;  /* 重置计数器 */
            return newValue;  /* 确认变化，返回新值 */
        }
        else
        {
            return preValue;  /* 抖动期间，保持旧值 */
        }
    }
    else
    {
        /* 若相等则清零 */
        *pCount = 0;
        return newValue;
    }
}

/**
 * @brief 卡尔曼滤波算法（简化版）
 * @param curValue    当前测量值
 * @param pEstimate   估计值指针（静态变量）
 * @param pErrorEst   估计误差指针（静态变量）
 * @param q           过程噪声协方差
 * @param r           测量噪声协方差
 * @return            返回滤波后的值
 */
uint16_t kalmanFilter(const uint16_t curValue, float *pEstimate, float *pErrorEst, const float q, const float r)
{
    if (pEstimate == NULL || pErrorEst == NULL) return curValue;
    
    /* 预测步骤 */
    float predictEst = *pEstimate;
    float predictErr = *pErrorEst + q;
    
    /* 更新步骤 */
    float kalmanGain = predictErr / (predictErr + r);
    *pEstimate = predictEst + kalmanGain * ((float)curValue - predictEst);
    *pErrorEst = (1.0f - kalmanGain) * predictErr;
    
    return (uint16_t)(*pEstimate + 0.5f);  /* 四舍五入 */
}

/**
 * @brief 指数加权移动平均滤波（EWMA）
 * @param curValue  当前值
 * @param pLastAvg  上次平均值指针（静态变量）
 * @param alpha     平滑因子 (0 < alpha <= 1)
 * @return          返回滤波后的值
 */
uint16_t ewmaFilter(const uint16_t curValue, float *pLastAvg, const float alpha)
{
    if (pLastAvg == NULL || alpha <= 0.0f || alpha > 1.0f) return curValue;
    
    if (*pLastAvg == 0.0f)
    {
        *pLastAvg = (float)curValue;  /* 初始化 */
    }
    else
    {
        *pLastAvg = alpha * (float)curValue + (1.0f - alpha) * (*pLastAvg);
    }
    
    return (uint16_t)(*pLastAvg + 0.5f);  /* 四舍五入 */
}

/**
 * @brief 双边滤波（去除突变噪声）
 * @param pValueArray 待滤波的数组
 * @param length      数组长度
 * @param threshold   突变检测阈值
 * @return            返回滤波后的中心值
 */
uint16_t bilateralFilter(uint16_t pValueArray[], const uint8_t length, const uint16_t threshold)
{
    if (pValueArray == NULL || length < 3) return (length > 0) ? pValueArray[0] : 0;
    
    uint8_t centerIdx = length / 2;
    uint16_t centerValue = pValueArray[centerIdx];
    uint16_t sum = 0;
    uint8_t count = 0;
    
    /* 只对与中心值差异小于阈值的点求平均 */
    for (uint8_t i = 0; i < length; i++)
    {
        uint16_t diff = (pValueArray[i] > centerValue) ? (pValueArray[i] - centerValue) : (centerValue - pValueArray[i]);
        if (diff <= threshold)
        {
            sum += pValueArray[i];
            count++;
        }
    }
    
    return (count > 0) ? sum / count : centerValue;
}

/**
 * @brief 自适应滤波（根据变化率调整滤波强度）
 * @param curValue    当前值
 * @param preValue    前一个值
 * @param pFilterVal  滤波值指针（静态变量）
 * @param minAlpha    最小滤波系数
 * @param maxAlpha    最大滤波系数
 * @return            返回自适应滤波后的值
 */
uint16_t adaptiveFilter(const uint16_t curValue, const uint16_t preValue, float *pFilterVal, const float minAlpha, const float maxAlpha)
{
    if (pFilterVal == NULL) return curValue;
    
    if (*pFilterVal == 0.0f)
    {
        *pFilterVal = (float)curValue;  /* 初始化 */
        return curValue;
    }
    
    /* 计算变化率 */
    uint16_t change = (curValue > preValue) ? (curValue - preValue) : (preValue - curValue);
    float alpha = minAlpha;
    
    /* 根据变化率调整滤波系数 */
    if (change > 20)  /* 大变化，使用较大的alpha（响应快） */
    {
        alpha = maxAlpha;
    }
    else if (change > 10)  /* 中等变化 */
    {
        alpha = (minAlpha + maxAlpha) / 2.0f;
    }
    /* 小变化使用minAlpha（平滑效果好） */
    
    *pFilterVal = alpha * (float)curValue + (1.0f - alpha) * (*pFilterVal);
    
    return (uint16_t)(*pFilterVal + 0.5f);  /* 四舍五入 */
}

/* ---------------------------------------- 工具函数 ---------------------------------------- */

/**
 * @brief 数组限幅滤波
 * @param array1     输入数组
 * @param array2     输出数组
 * @param maxLimit   限幅范围
 * @param dataLength 数据长度
 * @return           无
 */
void limitFilterArray(uint16_t *array1, uint16_t *array2, uint16_t maxLimit, uint8_t dataLength)
{
    if (array1 == NULL || array2 == NULL || dataLength == 0) return;
    
    uint16_t preValue = array1[0];
    array2[0] = array1[0];
    
    for (uint8_t i = 1; i < dataLength; i++)
    {
        uint16_t diff = (array1[i] > preValue) ? (array1[i] - preValue) : (preValue - array1[i]);
        if (diff > maxLimit)
        {
            array2[i] = preValue;
        }
        else
        {
            array2[i] = array1[i];
        }
        preValue = array2[i];
    }
}
